<!DOCTYPE html>
<html>

<head>
  <meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1, user-scalable=no">
  <meta charset="utf-8" />

  <title>Simple URDF Loading Example</title>

  <style>
    html,
    body {
      box-sizing: border-box;
      margin: 0;
      padding: 0;
      overflow: hidden;
    }

    #cameraB-view {
      position: absolute;
      bottom: 20px;
      right: 20px;
      width: 200px;
      height: 200px;
      border: 2px solid white;
      background: #000;
    }

    #data-view {
      position: absolute;
      top: 20px;
      right: 20px;
      width: 200px;
      height: 200px;
      border: 1px solid white;
      background: #000;
      color: white;
      opacity: 0.5;
      padding: 5px;
    }    
  </style>
  <script type="application/javascript" src="../jquery-3.7.1.min.js"></script>
  <script type="application/javascript" src="../reconnectwebsocket.js"></script>
  <script type="importmap">
      {
        "imports": {
          "three": "../build/three.module.js",
          "three/addons/": "../examples/jsm/"
        }
      }
    </script>
  <script>
    var scene, camera, renderer, controls;
    var robot;
    var cachePanels = [];
    var Panels = [];
    var ws = null;

    var newMaterial1, newMaterial2, newMaterial3, newMaterial0;
    var cameraBRenderer;
    var cameraB;
    var cameraBHelper;
    var sendCameraBImage;

    var lineGeometry;
    var cameraSphereContainer;

  </script>
  <script type="module">
    import {
      WebGLRenderer,
      Scene,
      Mesh,
      PlaneGeometry,
      ShadowMaterial,
      DirectionalLight,
      BufferGeometry,
      Line,
      LineBasicMaterial,
      PCFSoftShadowMap,
      MeshBasicMaterial,
      Object3D,
      sRGBEncoding,
      SphereGeometry,
      MeshPhongMaterial,
      BufferAttribute,
      GridHelper,
      Color,
      AmbientLight,
      Box3,
      LoadingManager,
      MathUtils,
      ArrowHelper,
      Vector3,
      PerspectiveCamera,
      CameraHelper,
    } from 'three';
    import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
    import URDFLoader from 'three/addons/loaders/URDFLoader.js';




    setTimeout(function () {
      console.debug(111);
      init();
      render();

    }, 1000);



    function init() {

      scene = new Scene();
      scene.background = new Color(0x263238);

      camera = new PerspectiveCamera();
      camera.position.set(-2, 2, 0);
      camera.lookAt(0, 0, 0);

      renderer = new WebGLRenderer({ antialias: true });
      renderer.outputEncoding = sRGBEncoding;
      renderer.shadowMap.enabled = true;
      renderer.shadowMap.type = PCFSoftShadowMap;
      document.body.appendChild(renderer.domElement);

      const directionalLight = new DirectionalLight(0xffffff, 1.0);
      directionalLight.castShadow = true;
      directionalLight.shadow.mapSize.setScalar(1024);
      directionalLight.position.set(5, 30, 5);
      scene.add(directionalLight);

      const ambientLight = new AmbientLight(0xffffff, 0.2);
      scene.add(ambientLight);

      const ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ opacity: 0.25 }));
      ground.rotation.x = -Math.PI / 2;
      ground.scale.setScalar(30);
      ground.receiveShadow = true;
      scene.add(ground);

      controls = new OrbitControls(camera, renderer.domElement);
      controls.minDistance = 1;
      //controls.target.y = 1;
      controls.update();

      // Load robot
      const manager = new LoadingManager();
      const loader = new URDFLoader(manager);
      const gridHelper = new GridHelper(20, 20, 0x888888, 0x444444);
      scene.add(gridHelper);

      loader.load('MJ0402X3/urdf/MJ0402X3_.urdf', result => {
        robot = result;
      });

      // wait until all the geometry has loaded to add the model to the scene
      manager.onLoad = () => {
        robot.rotation.x = Math.PI / 2;
        robot.rotation.x *= (-1);
        robot.position.x = 0;
        robot.position.z -= 1;
        robot.position.y = 0;

        // robot.links.l_pcr_3.children[0].visible = false;
        // robot.links.l_pcr_4.children[0].visible = false;

        robot.updateMatrixWorld(true);

        //const bb = new Box3();
        //bb.setFromObject(robot);

        //robot.position.y -= bb.min.y;
        scene.add(robot);

        const fixedRayLength = 0.05; // 20mm = 2cm

        // 创建射线辅助可视化(固定长度20mm)
        let rayHelper = new ArrowHelper(
          new Vector3(-1, 0, 0),
          new Vector3(-0.1, -0.04, -0.01),
          fixedRayLength,
          0x00ff00,
          0.005, // 头部长度
          0.005  // 头部宽度
        );
        //robot.joints['j_obj'].add(rayHelper);

        cameraB = new PerspectiveCamera(15, 1, 0.1, 0.2);
        // 将摄像头放在立方体前方边缘
        cameraB.position.set(0.02, -0.06, -0.008);

        cameraB.lookAt(-1, 0, 0); // 看向前方

        // 将摄像头添加到立方体，使其随立方体移动
        robot.joints['j_obj'].add(cameraB);

        // 创建CameraHelper并添加到场景
        cameraBHelper = new CameraHelper(cameraB);
        scene.add(cameraBHelper);

        // 为cameraB创建单独的渲染器
        cameraBRenderer = new WebGLRenderer({ antialias: true, preserveDrawingBuffer: true });
        cameraBRenderer.setSize(200, 200);
        document.getElementById('cameraB-view').appendChild(cameraBRenderer.domElement);



        // ==== 新增部分：创建两个小球和连线 ====
        // 在 (0,0,0) 处创建一个小球（红色）
        const originSphere = new Mesh(
          new SphereGeometry(0.01, 16, 16),
          new MeshBasicMaterial({ color: 0xff0000 })
        );
        scene.add(originSphere); // 直接添加到场景，不受相机父级影响

        // 在 cameraB.position 处创建一个小球（蓝色）
        const cameraSphere = new Mesh(
          new SphereGeometry(0.01, 16, 16),
          new MeshBasicMaterial({ color: 0x0000ff })
        );
        // 注意：由于 cameraB 可能移动，我们需要创建一个独立于相机的球体
        cameraSphereContainer = new Object3D();
        cameraSphereContainer.position.copy(cameraB.position);
        scene.add(cameraSphereContainer);
        cameraSphereContainer.add(cameraSphere);

        // 创建连接线（绿色）
        lineGeometry = new BufferGeometry();
        const lineMaterial = new LineBasicMaterial({ color: 0x00ff00 });
        const line = new Line(lineGeometry, lineMaterial);
        scene.add(line);

      };

      onResize();
      window.addEventListener('resize', onResize);


    }

    function updateLine() {
  // 获取相机在世界坐标系中的位置
  cameraB.getWorldPosition(cameraSphereContainer.position);
  
  // 更新线条
  const positions = new Float32Array([
    0, 0, 0, // 起点 (0,0,0)
    cameraSphereContainer.position.x,
    cameraSphereContainer.position.y,
    cameraSphereContainer.position.z // 终点 (相机位置)
  ]);
  lineGeometry.setAttribute('position', new BufferAttribute(positions, 3));
  lineGeometry.attributes.position.needsUpdate = true;
}    

    function onResize() {

      renderer.setSize(window.innerWidth, window.innerHeight);
      renderer.setPixelRatio(window.devicePixelRatio);

      camera.aspect = window.innerWidth / window.innerHeight;
      camera.updateProjectionMatrix();

    }

    function render() {

      requestAnimationFrame(render);
      renderer.render(scene, camera);

      // 更新cameraBHelper以反映cameraB的变化
      if (cameraBHelper) {
        cameraBHelper.update();
      }
      if(typeof(cameraBRenderer)!='undefined'){
        cameraBRenderer.render(scene, cameraB);
        updateLine();
      }
      

    }

  </script>
  <script type="application/javascript" src="websocket_data.js"></script>
  <script type="application/javascript" src="websocket_components_display.js"></script>
  <script type="application/javascript" src="websocket_actions.js"></script>
</head>

<body>
  <div id="cameraB-view"></div>
  <div id="data-view">
    j1:<span id="j1"></span><br/>
    j2:<span id="j2"></span><br/>
    j3:<span id="j3"></span><br/>
    image:<span id="image"></span><br/>
  </div>
</body>

</html>